/*
 * Copyright (c) 2019, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-09-13     WangShun     The first version
 */

#include "example.h"
#include <aos/aos.h>
#include "aos/cli.h"

motor_t motor_dev;
servo_t servo;
single_phase_encoder_t encoder;

void robot_example(int argc, char **argv)
{
    if (argc > 1)
    {
        if(!strcmp(*(argv+1), "help"))
        {
            aos_cli_printf("Command list:\r\n");
            aos_cli_printf("##single_motor\r\n");
            aos_cli_printf("<start>: robot_example single_motor start <val:0~100> #(val%)\r\n");
            aos_cli_printf("<stop>:  robot_example single_motor stop \r\n");
            aos_cli_printf("##dual_motor\r\n");
            aos_cli_printf("<start>: robot_example dual_motor start <val:0~100> #(val%)\r\n");
            aos_cli_printf("<stop>:  robot_example dual_motor stop \r\n");
            aos_cli_printf("##servo\r\n");
            aos_cli_printf("<start>: robot_example servo start <angle:0 ~ 180> #(angle°)\r\n");
            aos_cli_printf("<stop>:  robot_example servo stop\r\n");
            aos_cli_printf("##encoder\r\n");
            aos_cli_printf("<start>: robot_example encoder start\r\n");
            aos_cli_printf("<stop>:  robot_example encoder stop \r\n");                   
            aos_cli_printf("List end\r\n");
        }
        else if(!strcmp(*(argv+1), "single_motor"))
        {
            if(argc > 3)
            {
                if (!strcmp(*(argv+2), "start"))
                {
                    csi_pin_set_mux(PB5, PB5_PWM0);
                    motor_dev = (motor_t) single_pwm_motor_create("pwm0", 0, PE11, PE16);
                    motor_run(motor_dev, atoi(argv[3]));
                    motor_enable(motor_dev);
                }
                else if (!strcmp(*(argv+2), "stop"))
                {
                    motor_disable(motor_dev);
                }
            }
            else
                goto end;
        }
        else if(!strcmp(*(argv+1), "dual_motor"))
        {
            if(argc > 3)
            {
                if (!strcmp(*(argv+2), "start"))
                {
                    csi_pin_set_mux(PB5, PB5_PWM0);
                    csi_pin_set_mux(PB6, PB6_PWM1);
                    motor_dev = (motor_t)dual_pwm_motor_create("pwm0", 0, "pwm0", 1);
                    motor_run(motor_dev, atoi(argv[3]));
                    motor_enable(motor_dev);

                }
                else if (!strcmp(*(argv+2), "stop"))
                {
                    motor_disable(motor_dev);
                }
            }
            else
                goto end;
        }
        else if(!strcmp(*(argv+1), "servo"))
        {
            if(argc > 3)
            {
                if (!strcmp(*(argv+2), "start"))
                {
                    csi_pin_set_mux(PB5, PB5_PWM0);
                    servo = servo_create("pwm0", 0, 180.0, 0.5, 2.5);
                    servo_set_angle(servo, atoi(argv[3]));
                    servo_enable(servo);
                }
                else if (!strcmp(*(argv+2), "stop"))
                {
                    servo_disable(servo);
                }  
            }
            else
                goto end;
        }
        else if(!strcmp(*(argv+1), "encoder"))
        {  
            int value, num, i = 0;
            static int last_num = 0, flag = 0; 
            if(argc > 2)
            {   
                if (!strcmp(*(argv+2), "start"))
                {
                    if (flag == 0)
                    {
                        csi_pin_set_mux(PE5, PIN_FUNC_GPIO);
                        encoder = single_phase_encoder_create(PE5, 100, 100);
                        encoder_enable((encoder_t)encoder); 
                    }
                    while (1)
                    {
                        i++;
                        num = encoder_read((encoder_t)encoder);
                        value  = num -last_num;
                        last_num = num;
                        if (i < 20)
                        {
                            aos_cli_printf("num = %d\r\n", value);
                            aos_msleep(5);                       
                        }
                        else
                        {
                            break;    
                        }
                    }
                    flag = 1;
                }
                else if (!strcmp(*(argv+2), "stop"))
                {
                    encoder_disable((encoder_t)encoder);
                }
            }
            else
                goto end;
        }  
    }
    else
    {
end:
        aos_cli_printf("Unknown command. Please enter 'robot_example help' for help\n");
    }
}
ALIOS_CLI_CMD_REGISTER(robot_example, robot_example, robot example);